#ifndef COMPASS_H
#define COMPASS_H

#include <stdint.h>
//#include "algebra.h"
#include <Vector3.h>
#include <Matrix3.h>


struct compass_setting_t
{
		uint8_t mark;
		int16_t zero[3];
		
		void init()
		{
			  mark = 'C';
			  zero[0] = zero[1] = zero[2] = 0;
		} 
};


class Compass
{
public:
    void init();
    void update();
    void calibrate();
    inline bool ready()
    {
        return t_cal == 0;
    }
    
    void load_setting(void);
    
    inline void save_setting();
    
    /*float get_heading()
    {
        return heading;
    }*/
    
    void calculate(const Matrix3f& dcm);
    void calculate(float roll, float pitch);
    
//    inline Vector3f& get_vector()
//    {
//        return vector;
//    }
public:
    inline void read(int16_t out[]);
    //inline void rotate_sensor(int16_t x, int16_t y, int16_t z);
    //int16_t raw_data[3];
    //int16_t data[3];
    float heading;
    float heading_x;
    float heading_y;
private:
    compass_setting_t setting;
    int16_t data[3];
    int32_t t_cal;
    //bool start_smooth;
    union
    {
        struct
        {
            int16_t min[3];
            int16_t max[3];
        } ext;
        int16_t sum[3];
    } sm;

    //Vector3f vector;
};

#endif
